/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_joy
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
 		                          INCLUDES
 *****************************************************************************/

// General
#include <cmath>

// Schunk_joy
#include <schunk_joy/schunk_joy_simple.h>

// Service Clients


/******************************************************************************
 		                         MAIN ROUTINE
 *****************************************************************************/

int main(int argc, char** argv)
{
	// Initialize ROS, specify name of node
	ros::init(argc, argv, "schunk_joy");

	// Create SchunkJoystick object
	SchunkJoystick joystick;
	joystick.GetButtonConfiguration();

	while(joystick.nodehandle_.ok())
	{
		joystick.joy_queue_.callAvailable((ros::WallDuration()));
	}

	return 0;
}

/******************************************************************************
      	  	  	  	  	  CONSTRUCTORS AND DESTRUCTORS
 *****************************************************************************/

SchunkJoystick::SchunkJoystick()
{
	// Assignment of the defined callback queues to each node handler
	service_client_nodehandle_.setCallbackQueue(&service_client_queue_);
	publisher_nodehandle_.setCallbackQueue(&publisher_queue_);
	joy_nodehandle_.setCallbackQueue(&joy_queue_);
	position_nodehandle_.setCallbackQueue(&position_queue_);

	// Implementation of topics to publish
	publisher_joint_ = publisher_nodehandle_.advertise<schunk_msgs::JointVelocities>("/schunk_high/joint_controller/references", 1);

	// Implementation of topics to subscribe
	subscriber_joy_references_ = joy_nodehandle_.subscribe("/joy", 1, &SchunkJoystick::JoyCallback, this);

	// Implementation of service clients
	service_client_joint_init_control_ = service_client_nodehandle_.serviceClient<schunk_high::Init>("schunk_high/joint_controller/init");
	service_client_joint_enable_control_ = service_client_nodehandle_.serviceClient<schunk_high::Enable>("schunk_high/joint_controller/enable_control");
	service_client_joint_disable_control_ = service_client_nodehandle_.serviceClient<schunk_high::Disable>("schunk_high/joint_controller/disable_control");

	service_client_gripper_set_vel_ = service_client_nodehandle_.serviceClient<schunk_low::GripperSetVel>("schunk_low/gripper/set_velocity");
	service_client_gripper_open_ = service_client_nodehandle_.serviceClient<schunk_low::GripperOpen>("schunk_low/gripper/open");

	// Setting status variables
	connected_ = false;
	moving_gripper_ = false;
	moving_robot_ = false;
	selected_control_set_ = 0;
	selected_controller_ = JOINT_CONTROLLER;

	joint_controller_available_ = false;

	message_joint_.velocities.resize(6);

	max_velocity_ = 0.5;
	gripper_velocity_ = 0.01;

	initial_position_read_ = false;
}

SchunkJoystick::~SchunkJoystick()
{

}

/******************************************************************************
      	  	  	  	  	   PARAMETER LOADING FUNCTIONS
 *****************************************************************************/

bool SchunkJoystick::GetButtonConfiguration()
{
	bool error = false;

	// Dead man button
    if (nodehandle_.hasParam("/schunk_joy/button/dead_man"))
    {
    	nodehandle_.getParam("/schunk_joy/button/dead_man", button_dead_man_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_dead_man not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

	// Init button
    if (nodehandle_.hasParam("/schunk_joy/button/init"))
    {
    	nodehandle_.getParam("/schunk_joy/button/init", button_init_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_init not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

	// Open gripper button
    if (nodehandle_.hasParam("/schunk_joy/button/grip_open"))
    {
    	nodehandle_.getParam("/schunk_joy/button/grip_open", button_grip_open_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_grip_open not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

	// Close gripper button
    if (nodehandle_.hasParam("/schunk_joy/button/grip_close"))
    {
    	nodehandle_.getParam("/schunk_joy/button/grip_close", button_grip_close_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_grip_close not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

	// Select control set 1 button
    if (nodehandle_.hasParam("/schunk_joy/button/control_set_0"))
    {
    	nodehandle_.getParam("/schunk_joy/button/control_set_0", button_control_set_0_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_control_set_0 not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

	// Select control set 2 button
    if (nodehandle_.hasParam("/schunk_joy/button/control_set_1"))
    {
    	nodehandle_.getParam("/schunk_joy/button/control_set_1", button_control_set_1_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_control_set_1 not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

	// Select control set 3 button
    if (nodehandle_.hasParam("/schunk_joy/button/control_set_2"))
    {
    	nodehandle_.getParam("/schunk_joy/button/control_set_2", button_control_set_2_);
    }
    else
    {
    	ROS_ERROR_STREAM("Parameter button_control_set_2 not set, shutting down node...");
    	nodehandle_.shutdown();
    	error = true;
    }

    return !error;
}

/******************************************************************************
      	  	  	  	  	  SUBSCRIBED TOPIC CALLBACKS
 *****************************************************************************/

void SchunkJoystick::JoyCallback(const sensor_msgs::Joy::ConstPtr& msg)
{

	// Priority 1 Button : Init - Try initializing the controller and check which are available
	if(msg->buttons[button_init_] == 1)
	{
		ROS_INFO_STREAM("Initializing Schunk.");

		service_client_joint_init_control_.call(service_init_message_);

		ROS_INFO_STREAM("Enabling joint controller.");
        if(!service_client_joint_enable_control_.call(service_enable_message_))
        {
            ROS_ERROR_STREAM("Could not enable the joint controller.");
        }
        else
        {
            connected_ = true;
            ROS_INFO_STREAM("Joint controller was enabled.");
        }

	}

	// Priority 3 Button : Control Set Switcher
    else if(ButtonSwitchControlSetPressed(msg))
    {


        if(connected_)
        {
            if(msg->buttons[button_control_set_0_] == 1) selected_control_set_ = 0;
            else if(msg->buttons[button_control_set_1_] == 1) selected_control_set_ = 1;
            else if(msg->buttons[button_control_set_2_] == 1) selected_control_set_ = 2;
        	ROS_INFO_STREAM("Changing control set to control set #" << selected_control_set_);
        }
    }


	// Priority 3 Button : Dead Man Button - Move the robot
	else if(msg->buttons[button_dead_man_] == 1)
	{
		if(connected_)
				CheckJoystickAxesAndMoveRobot(msg);
	}

	else
	{
		if(connected_)
		{
			if(moving_robot_)
					SendZeroJointVelocitiesToTheRobot();
		}
	}

	// Moving the gripper - This checking is independent of the previous ifs
	if(msg->buttons[button_grip_close_] == 1)
	{
		service_grip_vel_message_.request.velocity = -gripper_velocity_;
		service_client_gripper_set_vel_.call(service_grip_vel_message_);
		moving_gripper_ = true;
	}

	else if(msg->buttons[button_grip_open_] == 1)
	{
		service_grip_vel_message_.request.velocity = gripper_velocity_;
		service_client_gripper_set_vel_.call(service_grip_vel_message_);
		moving_gripper_ = true;
	}

	else if(moving_gripper_ && msg->buttons[button_grip_close_] == 0 && msg->buttons[button_grip_open_] == 0)
	{
		service_grip_vel_message_.request.velocity = 0.0;
		service_client_gripper_set_vel_.call(service_grip_vel_message_);
		moving_gripper_ = false;
	}
}

/******************************************************************************
      	  	  	  	  	     AUXILIAR FUNCTIONS
 *****************************************************************************/
inline bool SchunkJoystick::ButtonSwitchControlSetPressed(const sensor_msgs::Joy::ConstPtr& msg)
{
    if(msg->buttons[button_control_set_0_] == 1) return true;
    else if(msg->buttons[button_control_set_1_] == 1) return true;
    else if(msg->buttons[button_control_set_2_] == 1) return true;
    else return false;
}

void SchunkJoystick::CheckJoystickAxesAndMoveRobot(const sensor_msgs::Joy::ConstPtr& msg)
{
	if(selected_controller_ == JOINT_CONTROLLER)
	{
		for(int i = 0; i < (int)message_joint_.velocities.size(); i++)
			message_joint_.velocities[i] = 0.0;

		message_joint_.velocities[2*selected_control_set_] = -max_velocity_ * msg->axes[2];
		message_joint_.velocities[2*selected_control_set_ + 1] = max_velocity_ * msg->axes[1];

		message_joint_.header.stamp = ros::Time::now();
		publisher_joint_.publish(message_joint_);
		publisher_queue_.callAvailable(ros::WallDuration(0));

		moving_robot_ = true;
	}
}

inline void SchunkJoystick::SendZeroJointVelocitiesToTheRobot()
{
	for(int i = 0; i < (int)message_joint_.velocities.size(); i++)
		message_joint_.velocities[i] = 0.0;
	message_joint_.header.stamp = ros::Time::now();
	publisher_joint_.publish(message_joint_);
	publisher_queue_.callAvailable(ros::WallDuration(0));

	moving_robot_ = false;
}
